function grid_map = get_grid_map(points,grid_num)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% -input args:
%     points: n*5的矩阵
%     grid_num: 用于将点云在x-y平面上分格的个数，如100即为分为100*100格方格
% -output args:
%     grid_map: 结构体数组，维度为grid_num*grid_num，记录每个格点内的信息。如下：
%         point_num：当前格点内点云个数。
%         points: 当前格点内的点云信息，维度为5.若无点云则为[]
%         dp: 一个[(point_num-1) * 1]的向量，记录将格点内点云在z轴排序后，两两之间的间隔
%         maxh:  格点内最大高度
%         minh： 格点内最低高度
%         dh：   格点内最大高度-最小高度
%         aveh： 格点内平均高度
%         medh： 格点内高度中位数
%         hmap： 调试用。即再对每一格点内高度值做100格量化。没啥用
%         gh：   地面高度（估计）。对于格点内存在大间隔的格点（最大dp大于0.05），
%                   选取间隔以下的点的平均高度。对于不存在大间隔的点，选取高度值最低的1/3点的平均高度
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    minx = min(points(:,1));
    maxx = max(points(:,1))+0.001;
    miny = min(points(:,2));
    maxy = max(points(:,2))+0.001;
    step_x = (maxx-minx)/grid_num;
    step_y = (maxy-miny)/grid_num;
    grid_map(grid_num,grid_num).point_num = 0;
    disp('Pre-setting Done.');
    disp(['Total Point Number:',num2str(length(points))]);
    px = sortrows(points,1);
    c =0;
    for i = 1:grid_num
        left = minx+(i-1)*step_x;
        right = left+step_x;
        msk = (px(:,1)>=left) & (px(:,1)<right);
        mskl = find(msk,1,'first');
        mskr = find(msk,1,'last');
        py = sortrows(px(mskl:mskr,:),2);
        for j = 1:grid_num
            up = miny+(j-1)*step_y;
            down = up+step_y;
            msky = (py(:,2)>=up) & (py(:,2)<down);
            msku = find(msky,1,'first');
            mskd = find(msky,1,'last');
            if ~isempty(msku) && ~isempty(mskd)
                grid_map(i,j).points = py(msku:mskd,:);
                grid_map(i,j).point_num = size(grid_map(i,j).points,1);
            else
                grid_map(i,j).point_num = 0;
            end
            if (grid_map(i,j).point_num>0)
                c = c + grid_map(i,j).point_num;
                disp(['Processed: ',num2str(c),'/',num2str(length(points))]);
            end
        end
    end

    disp('Grid_map generation done.');
    for i = 1:grid_num
        for j = 1:grid_num
            if grid_map(i,j).point_num>1
                grid_map(i,j).points = grid_map(i,j).points(1:grid_map(i,j).point_num,:);
                p = sortrows(grid_map(i,j).points,3);
                dp = diff(p(:,3));
                grid_map(i,j).dp = dp;
                [m,ind] = max(dp);
                grid_map(i,j).maxh = max(grid_map(i,j).points(:,3));
                grid_map(i,j).minh = min(grid_map(i,j).points(:,3));
                grid_map(i,j).dh = grid_map(i,j).maxh-grid_map(i,j).minh;
                grid_map(i,j).aveh = sum(grid_map(i,j).points(:,3))/grid_map(i,j).point_num;
                grid_map(i,j).medh = median(grid_map(i,j).points(:,3));
                grid_map(i,j).hmap =zeros(100,1);
                grid_map(i,j).gh=0;
                sh = (grid_map(i,j).maxh-grid_map(i,j).minh)/100;
                for k = 1:100
                    low = grid_map(i,j).minh+(k-1)*sh;
                    high = low+sh;
                    msk = (p(:,3)>=low) & (p(:,3)<high);
                    mskd = find(msk,1,'first');
                    msku = find(msk,1,'last');

                    grid_map(i,j).hmap(k) = grid_map(i,j).hmap(k)+size(p(mskd:msku,:),1);
                end
                if (m>0.05)
                    if (ind>1) && ~isempty(ind)
                        grid_map(i,j).gh = p(ind,3);
                    end
                else
                    ll = ceil(size(p,1)/3);
                    grid_map(i,j).gh = mean(p(1:ll,3));
                end
            else 
                grid_map(i,j).points = [];
                grid_map(i,j).maxh =0;
                grid_map(i,j).minh =0;
                grid_map(i,j).dh =0;
                grid_map(i,j).aveh=0;
                grid_map(i,j).medh=0;
                grid_map(i,j).dp = [];
                grid_map(i,j).gh = 0;
            end
        end
    end
end